# pragma once

#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <chrono>
#include <thread>
#include <ostream>
#include "serialib.h"

// 数据类型定义
typedef enum {
    MC_STATE_OFF = 0,
    MC_STATE_DETECTING,
    MC_STATE_RUNNING,
    MC_STATE_FULL_BRAKE,
} mc_state;

typedef enum {
    PWM_MODE_NONSYNCHRONOUS_HISW = 0,
    PWM_MODE_SYNCHRONOUS,
    PWM_MODE_BIPOLAR
} mc_pwm_mode;

typedef enum {
    COMM_MODE_INTEGRATE = 0,
    COMM_MODE_DELAY
} mc_comm_mode;

typedef enum {
    SENSOR_MODE_SENSORLESS = 0,
    SENSOR_MODE_SENSORED,
    SENSOR_MODE_HYBRID
} mc_sensor_mode;

typedef enum {
    FOC_SENSOR_MODE_SENSORLESS = 0,
    FOC_SENSOR_MODE_ENCODER,
    FOC_SENSOR_MODE_HALL
} mc_foc_sensor_mode;

typedef enum {
    OUT_AUX_MODE_OFF = 0,
    OUT_AUX_MODE_ON_AFTER_2S,
    OUT_AUX_MODE_ON_AFTER_5S,
    OUT_AUX_MODE_ON_AFTER_10S
} out_aux_mode;

typedef enum {
    MOTOR_TYPE_BLDC = 0,
    MOTOR_TYPE_DC,
    MOTOR_TYPE_FOC
} mc_motor_type;

typedef enum {
    FAULT_CODE_NONE = 0,
    FAULT_CODE_OVER_VOLTAGE,
    FAULT_CODE_UNDER_VOLTAGE,
    FAULT_CODE_DRV,
    FAULT_CODE_ABS_OVER_CURRENT,
    FAULT_CODE_OVER_TEMP_FET,
    FAULT_CODE_OVER_TEMP_MOTOR
} mc_fault_code;

typedef enum {
    CONTROL_MODE_DUTY = 0,
    CONTROL_MODE_SPEED,
    CONTROL_MODE_CURRENT,
    CONTROL_MODE_CURRENT_BRAKE,
    CONTROL_MODE_POS,
    CONTROL_MODE_HANDBRAKE,
    CONTROL_MODE_OPENLOOP,
    CONTROL_MODE_NONE
} mc_control_mode;

typedef enum {
    DISP_POS_MODE_NONE = 0,
    DISP_POS_MODE_INDUCTANCE,
    DISP_POS_MODE_OBSERVER,
    DISP_POS_MODE_ENCODER,
    DISP_POS_MODE_PID_POS,
    DISP_POS_MODE_PID_POS_ERROR,
    DISP_POS_MODE_ENCODER_OBSERVER_ERROR
} disp_pos_mode;

typedef enum {
    SENSOR_PORT_MODE_HALL = 0,
    SENSOR_PORT_MODE_ABI,
    SENSOR_PORT_MODE_AS5047_SPI
} sensor_port_mode;

typedef struct {
    float cycle_int_limit;
    float cycle_int_limit_running;
    float cycle_int_limit_max;
    float comm_time_sum;
    float comm_time_sum_min_rpm;
    int32_t comms;
    uint32_t time_at_comm;
} mc_rpm_dep_struct;

typedef enum {
    DRV8301_OC_LIMIT = 0,
    DRV8301_OC_LATCH_SHUTDOWN,
    DRV8301_OC_REPORT_ONLY,
    DRV8301_OC_DISABLED
} drv8301_oc_mode;

typedef enum {
    DEBUG_SAMPLING_OFF = 0,
    DEBUG_SAMPLING_NOW,
    DEBUG_SAMPLING_START,
    DEBUG_SAMPLING_TRIGGER_START,
    DEBUG_SAMPLING_TRIGGER_FAULT,
    DEBUG_SAMPLING_TRIGGER_START_NOSEND,
    DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND,
    DEBUG_SAMPLING_SEND_LAST_SAMPLES
} debug_sampling_mode;

typedef enum {
    CAN_BAUD_125K = 0,
    CAN_BAUD_250K,
    CAN_BAUD_500K,
    CAN_BAUD_1M
} CAN_BAUD;

typedef struct {
    mc_pwm_mode pwm_mode;
    mc_comm_mode comm_mode;
    mc_motor_type motor_type;
    mc_sensor_mode sensor_mode;
    float l_current_max;
    float l_current_min;
    float l_in_current_max;
    float l_in_current_min;
    float l_abs_current_max;
    float l_min_erpm;
    float l_max_erpm;
    float l_erpm_start;
    float l_max_erpm_fbrake;
    float l_max_erpm_fbrake_cc;
    float l_min_vin;
    float l_max_vin;
    float l_battery_cut_start;
    float l_battery_cut_end;
    bool l_slow_abs_current;
    float l_temp_fet_start;
    float l_temp_fet_end;
    float l_temp_motor_start;
    float l_temp_motor_end;
    float l_temp_accel_dec;
    float l_min_duty;
    float l_max_duty;
    float l_watt_max;
    float l_watt_min;
    float lo_current_max;
    float lo_current_min;
    float lo_in_current_max;
    float lo_in_current_min;
    float lo_current_motor_max_now;
    float lo_current_motor_min_now;
    float sl_min_erpm;
    float sl_min_erpm_cycle_int_limit;
    float sl_max_fullbreak_current_dir_change;
    float sl_cycle_int_limit;
    float sl_phase_advance_at_br;
    float sl_cycle_int_rpm_br;
    float sl_bemf_coupling_k;
    int8_t hall_table[8];
    float hall_sl_erpm;
    float foc_current_kp;
    float foc_current_ki;
    float foc_f_sw;
    float foc_dt_us;
    float foc_encoder_offset;
    bool foc_encoder_inverted;
    float foc_encoder_ratio;
    float foc_motor_l;
    float foc_motor_r;
    float foc_motor_flux_linkage;
    float foc_observer_gain;
    float foc_observer_gain_slow;
    float foc_pll_kp;
    float foc_pll_ki;
    float foc_duty_dowmramp_kp;
    float foc_duty_dowmramp_ki;
    float foc_openloop_rpm;
    float foc_sl_openloop_hyst;
    float foc_sl_openloop_time;
    float foc_sl_d_current_duty;
    float foc_sl_d_current_factor;
    mc_foc_sensor_mode foc_sensor_mode;
    uint8_t foc_hall_table[8];
    float foc_sl_erpm;
    bool foc_sample_v0_v7;
    bool foc_sample_high_current;
    float foc_sat_comp;
    bool foc_temp_comp;
    float foc_temp_comp_base_temp;
    float foc_current_filter_const;
    float s_pid_kp;
    float s_pid_ki;
    float s_pid_kd;
    float s_pid_kd_filter;
    float s_pid_min_erpm;
    bool s_pid_allow_braking;
    float p_pid_kp;
    float p_pid_ki;
    float p_pid_kd;
    float p_pid_kd_filter;
    float p_pid_ang_div;
    float cc_startup_boost_duty;
    float cc_min_current;
    float cc_gain;
    float cc_ramp_step_max;
    int32_t m_fault_stop_time_ms;
    float m_duty_ramp_step;
    float m_current_backoff_gain;
    uint32_t m_encoder_counts;
    sensor_port_mode m_sensor_port_mode;
    bool m_invert_direction;
    drv8301_oc_mode m_drv8301_oc_mode;
    int m_drv8301_oc_adj;
    float m_bldc_f_sw_min;
    float m_bldc_f_sw_max;
    float m_dc_f_sw;
    float m_ntc_motor_beta;
    out_aux_mode m_out_aux_mode;
} mc_configuration;

typedef enum {
    APP_NONE = 0,
    APP_PPM,
    APP_ADC,
    APP_UART,
    APP_PPM_UART,
    APP_ADC_UART,
    APP_NUNCHUK,
    APP_NRF,
    APP_CUSTOM
} app_use;

typedef enum {
    THR_EXP_EXPO = 0,
    THR_EXP_NATURAL,
    THR_EXP_POLY
} thr_exp_mode;

typedef enum {
    PPM_CTRL_TYPE_NONE = 0,
    PPM_CTRL_TYPE_CURRENT,
    PPM_CTRL_TYPE_CURRENT_NOREV,
    PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE,
    PPM_CTRL_TYPE_DUTY,
    PPM_CTRL_TYPE_DUTY_NOREV,
    PPM_CTRL_TYPE_PID,
    PPM_CTRL_TYPE_PID_NOREV
} ppm_control_type;

typedef struct {
    ppm_control_type ctrl_type;
    float pid_max_erpm;
    float hyst;
    float pulse_start;
    float pulse_end;
    float pulse_center;
    bool median_filter;
    bool safe_start;
    float throttle_exp;
    float throttle_exp_brake;
    thr_exp_mode throttle_exp_mode;
    float ramp_time_pos;
    float ramp_time_neg;
    bool multi_esc;
    bool tc;
    float tc_max_diff;
} ppm_config;

typedef enum {
    ADC_CTRL_TYPE_NONE = 0,
    ADC_CTRL_TYPE_CURRENT,
    ADC_CTRL_TYPE_CURRENT_REV_CENTER,
    ADC_CTRL_TYPE_CURRENT_REV_BUTTON,
    ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC,
    ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER,
    ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON,
    ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC,
    ADC_CTRL_TYPE_DUTY,
    ADC_CTRL_TYPE_DUTY_REV_CENTER,
    ADC_CTRL_TYPE_DUTY_REV_BUTTON,
    ADC_CTRL_TYPE_PID,
    ADC_CTRL_TYPE_PID_REV_CENTER,
    ADC_CTRL_TYPE_PID_REV_BUTTON
} adc_control_type;

typedef struct {
    adc_control_type ctrl_type;
    float hyst;
    float voltage_start;
    float voltage_end;
    float voltage_center;
    float voltage2_start;
    float voltage2_end;
    bool use_filter;
    bool safe_start;
    bool cc_button_inverted;
    bool rev_button_inverted;
    bool voltage_inverted;
    bool voltage2_inverted;
    float throttle_exp;
    float throttle_exp_brake;
    thr_exp_mode throttle_exp_mode;
    float ramp_time_pos;
    float ramp_time_neg;
    bool multi_esc;
    bool tc;
    float tc_max_diff;
    uint32_t update_rate_hz;
} adc_config;

typedef enum {
    CHUK_CTRL_TYPE_NONE = 0,
    CHUK_CTRL_TYPE_CURRENT,
    CHUK_CTRL_TYPE_CURRENT_NOREV
} chuk_control_type;

typedef struct {
    chuk_control_type ctrl_type;
    float hyst;
    float ramp_time_pos;
    float ramp_time_neg;
    float stick_erpm_per_s_in_cc;
    float throttle_exp;
    float throttle_exp_brake;
    thr_exp_mode throttle_exp_mode;
    bool multi_esc;
    bool tc;
    float tc_max_diff;
} chuk_config;

typedef enum {
    NRF_SPEED_250K = 0,
    NRF_SPEED_1M,
    NRF_SPEED_2M
} NRF_SPEED;

typedef enum {
    NRF_POWER_M18DBM = 0,
    NRF_POWER_M12DBM,
    NRF_POWER_M6DBM,
    NRF_POWER_0DBM,
    NRF_POWER_OFF
} NRF_POWER;

typedef enum {
    NRF_AW_3 = 0,
    NRF_AW_4,
    NRF_AW_5
} NRF_AW;

typedef enum {
    NRF_CRC_DISABLED = 0,
    NRF_CRC_1B,
    NRF_CRC_2B
} NRF_CRC;

typedef enum {
    NRF_RETR_DELAY_250US = 0,
    NRF_RETR_DELAY_500US,
    NRF_RETR_DELAY_750US,
    NRF_RETR_DELAY_1000US,
    NRF_RETR_DELAY_1250US,
    NRF_RETR_DELAY_1500US,
    NRF_RETR_DELAY_1750US,
    NRF_RETR_DELAY_2000US,
    NRF_RETR_DELAY_2250US,
    NRF_RETR_DELAY_2500US,
    NRF_RETR_DELAY_2750US,
    NRF_RETR_DELAY_3000US,
    NRF_RETR_DELAY_3250US,
    NRF_RETR_DELAY_3500US,
    NRF_RETR_DELAY_3750US,
    NRF_RETR_DELAY_4000US
} NRF_RETR_DELAY;

typedef struct {
    NRF_SPEED speed;
    NRF_POWER power;
    NRF_CRC crc_type;
    NRF_RETR_DELAY retry_delay;
    unsigned char retries;
    unsigned char channel;
    unsigned char address[3];
    bool send_crc_ack;
} nrf_config;

typedef struct {
    uint8_t controller_id;
    uint32_t timeout_msec;
    float timeout_brake_current;
    bool send_can_status;
    uint32_t send_can_status_rate_hz;
    CAN_BAUD can_baud_rate;
    app_use app_to_use;
    ppm_config app_ppm_conf;
    adc_config app_adc_conf;
    uint32_t app_uart_baudrate;
    chuk_config app_chuk_conf;
    nrf_config app_nrf_conf;
} app_configuration;

typedef enum {
    COMM_FW_VERSION = 0,
    COMM_JUMP_TO_BOOTLOADER,
    COMM_ERASE_NEW_APP,
    COMM_WRITE_NEW_APP_DATA,
    COMM_GET_VALUES,
    COMM_SET_DUTY,
    COMM_SET_CURRENT,
    COMM_SET_CURRENT_BRAKE,
    COMM_SET_RPM,
    COMM_SET_POS,
    COMM_SET_HANDBRAKE,
    COMM_SET_DETECT,
    COMM_SET_SERVO_POS,
    COMM_SET_MCCONF,
    COMM_GET_MCCONF,
    COMM_GET_MCCONF_DEFAULT,
    COMM_SET_APPCONF,
    COMM_GET_APPCONF,
    COMM_GET_APPCONF_DEFAULT,
    COMM_SAMPLE_PRINT,
    COMM_TERMINAL_CMD,
    COMM_PRINT,
    COMM_ROTOR_POSITION,
    COMM_EXPERIMENT_SAMPLE,
    COMM_DETECT_MOTOR_PARAM,
    COMM_DETECT_MOTOR_R_L,
    COMM_DETECT_MOTOR_FLUX_LINKAGE,
    COMM_DETECT_ENCODER,
    COMM_DETECT_HALL_FOC,
    COMM_REBOOT,
    COMM_ALIVE,
    COMM_GET_DECODED_PPM,
    COMM_GET_DECODED_ADC,
    COMM_GET_DECODED_CHUK,
    COMM_FORWARD_CAN,
    COMM_SET_CHUCK_DATA,
    COMM_CUSTOM_APP_DATA,
    COMM_NRF_START_PAIRING
} COMM_PACKET_ID;

typedef enum {
    CAN_PACKET_SET_DUTY = 0,
    CAN_PACKET_SET_CURRENT,
    CAN_PACKET_SET_CURRENT_BRAKE,
    CAN_PACKET_SET_RPM,
    CAN_PACKET_SET_POS,
    CAN_PACKET_FILL_RX_BUFFER,
    CAN_PACKET_FILL_RX_BUFFER_LONG,
    CAN_PACKET_PROCESS_RX_BUFFER,
    CAN_PACKET_PROCESS_SHORT_BUFFER,
    CAN_PACKET_STATUS,
    CAN_PACKET_SET_CURRENT_REL,
    CAN_PACKET_SET_CURRENT_BRAKE_REL,
    CAN_PACKET_SET_CURRENT_HANDBRAKE,
    CAN_PACKET_SET_CURRENT_HANDBRAKE_REL
} CAN_PACKET_ID;

typedef struct {
    mc_fault_code fault;
    float current;
    float current_filtered;
    float voltage;
    float duty;
    float rpm;
    int tacho;
    int cycles_running;
    int tim_val_samp;
    int tim_current_samp;
    int tim_top;
    int comm_step;
    float temperature;
    int drv8301_faults;
} fault_data;

typedef enum {
    LED_EXT_OFF = 0,
    LED_EXT_NORMAL,
    LED_EXT_BRAKE,
    LED_EXT_TURN_LEFT,
    LED_EXT_TURN_RIGHT,
    LED_EXT_BRAKE_TURN_LEFT,
    LED_EXT_BRAKE_TURN_RIGHT,
    LED_EXT_BATT
} LED_EXT_STATE;

typedef struct {
    int js_x;
    int js_y;
    int acc_x;
    int acc_y;
    int acc_z;
    bool bt_c;
    bool bt_z;
} chuck_data;

typedef struct {
    uint8_t js_x;
    uint8_t js_y;
    bool bt_c;
    bool bt_z;
    bool bt_push;
    float vbat;
} mote_state;

typedef enum {
    MOTE_PACKET_BATT_LEVEL = 0,
    MOTE_PACKET_BUTTONS,
    MOTE_PACKET_ALIVE,
    MOTE_PACKET_FILL_RX_BUFFER,
    MOTE_PACKET_FILL_RX_BUFFER_LONG,
    MOTE_PACKET_PROCESS_RX_BUFFER,
    MOTE_PACKET_PROCESS_SHORT_BUFFER,
    MOTE_PACKET_PAIRING_INFO
} MOTE_PACKET;

typedef struct {
    float v_in;
    float temp_mos1;
    float temp_mos2;
    float temp_mos3;
    float temp_mos4;
    float temp_mos5;
    float temp_mos6;
    float temp_pcb;
    float current_motor;
    float current_in;
    float rpm;
    float duty_now;
    float amp_hours;
    float amp_hours_charged;
    float watt_hours;
    float watt_hours_charged;
    int tachometer;
    int tachometer_abs;
    mc_fault_code fault_code;
} mc_values;

typedef enum {
    NRF_PAIR_STARTED = 0,
    NRF_PAIR_OK,
    NRF_PAIR_FAIL
} NRF_PAIR_RES;

// CRC函数
const unsigned short crc16_tab[] = { 0x0000, 0x1021, 0x2042, 0x3063, 0x4084,
    0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad,
    0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7,
    0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
    0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a,
    0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672,
    0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719,
    0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7,
    0x0840, 0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948,
    0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50,
    0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b,
    0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
    0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97,
    0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe,
    0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca,
    0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3,
    0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d,
    0xd31c, 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214,
    0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c,
    0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
    0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3,
    0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d,
    0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806,
    0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e,
    0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1,
    0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b,
    0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0,
    0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
    0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 };

unsigned short crc16(unsigned char *buf, unsigned int len) {
    unsigned int i;
    unsigned short cksum = 0;
    for (i = 0; i < len; i++) {
        cksum = crc16_tab[(((cksum >> 8) ^ *buf++) & 0xFF)] ^ (cksum << 8);
    }
    return cksum;
}

// Buffer函数
void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index) {
    buffer[(*index)++] = number >> 8;
    buffer[(*index)++] = number;
}

void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index) {
    buffer[(*index)++] = number >> 8;
    buffer[(*index)++] = number;
}

void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index) {
    buffer[(*index)++] = number >> 24;
    buffer[(*index)++] = number >> 16;
    buffer[(*index)++] = number >> 8;
    buffer[(*index)++] = number;
}

void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index) {
    buffer[(*index)++] = number >> 24;
    buffer[(*index)++] = number >> 16;
    buffer[(*index)++] = number >> 8;
    buffer[(*index)++] = number;
}

void buffer_append_float16(uint8_t* buffer, float number, float scale, int32_t *index) {
    buffer_append_int16(buffer, (int16_t)(number * scale), index);
}

void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index) {
    buffer_append_int32(buffer, (int32_t)(number * scale), index);
}

int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index) {
    int16_t res = ((uint16_t) buffer[*index]) << 8 |
                 ((uint16_t) buffer[*index + 1]);
    *index += 2;
    return res;
}

uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index) {
    uint16_t res = ((uint16_t) buffer[*index]) << 8 |
                  ((uint16_t) buffer[*index + 1]);
    *index += 2;
    return res;
}

int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index) {
    int32_t res = ((uint32_t) buffer[*index]) << 24 |
                 ((uint32_t) buffer[*index + 1]) << 16 |
                 ((uint32_t) buffer[*index + 2]) << 8 |
                 ((uint32_t) buffer[*index + 3]);
    *index += 4;
    return res;
}

uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index) {
    uint32_t res = ((uint32_t) buffer[*index]) << 24 |
                  ((uint32_t) buffer[*index + 1]) << 16 |
                  ((uint32_t) buffer[*index + 2]) << 8 |
                  ((uint32_t) buffer[*index + 3]);
    *index += 4;
    return res;
}

float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index) {
    return (float)buffer_get_int16(buffer, index) / scale;
}

float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index) {
    return (float)buffer_get_int32(buffer, index) / scale;
}

bool buffer_get_bool(const uint8_t *buffer, int32_t *index) {
    bool result = (buffer[*index] == 1);
    (*index)++;
    return result;
}

void buffer_append_bool(uint8_t *buffer, bool value, int32_t *index) {
    buffer[*index] = value ? 1 : 0;
    (*index)++;
}

/// @brief vesc 的 UART（串口）通信驱动
class VescUart {
public:
    struct dataPackage {
        float tempFET;
        float tempMotor;
        float avgMotorCurrent;
        float avgInputCurrent;
        float dutyCycleNow;
        long rpm;
        float inpVoltage;
        float ampHours;
        float ampHoursCharged;
        long tachometer;
        long tachometerAbs;
    };

protected:
    serialib serialPort;
    std::ostream* debugStream;

    dataPackage data;

    int receiveUartMessage(uint8_t * payloadReceived) {
        uint16_t counter = 0;
        uint16_t endMessage = 256;
        bool messageRead = false;
        uint8_t messageReceived[256];
        uint16_t lenPayload = 0;
        
        auto timeout = std::chrono::steady_clock::now() + std::chrono::milliseconds(100);

        while (std::chrono::steady_clock::now() < timeout && !messageRead) {
            int available = serialPort.available();
            while (available-- > 0 && counter < sizeof(messageReceived)) {
                char c;
                if (serialPort.readChar(&c) == 1) {
                    messageReceived[counter++] = static_cast<uint8_t>(c);

                    if (counter == 2) {
                        switch (messageReceived[0]) {
                            case 2:
                                endMessage = messageReceived[1] + 5;
                                lenPayload = messageReceived[1];
                                break;
                            case 3:
                                if (debugStream) {
                                    *debugStream << "Message is larger than 256 bytes - not supported" << std::endl;
                                }
                                return 0;
                            default:
                                if (debugStream) {
                                    *debugStream << "Invalid start bit" << std::endl;
                                }
                                return 0;
                        }
                    }

                    if (counter == endMessage && messageReceived[endMessage - 1] == 3) {
                        messageReceived[endMessage] = 0;
                        if (debugStream) {
                            *debugStream << "End of message reached!" << std::endl;
                        }
                        messageRead = true;
                        break;
                    }
                }
            }
        }

        if (!messageRead && debugStream) {
            *debugStream << "Timeout waiting for message" << std::endl;
        }
        
        if (messageRead) {
            return unpackPayload(messageReceived, endMessage, payloadReceived) ? lenPayload : 0;
        }
        
        return 0;
    }

    bool unpackPayload(uint8_t * message, int lenMes, uint8_t * payload) {
        uint16_t crcMessage = 0;
        uint16_t crcPayload = 0;

        crcMessage = static_cast<uint16_t>(message[lenMes - 3] << 8);
        crcMessage |= message[lenMes - 2];

        if (debugStream) {
            *debugStream << "CRC received: " << crcMessage << std::endl;
        }

        memcpy(payload, &message[2], message[1]);

        crcPayload = crc16(payload, message[1]);

        if (debugStream) {
            *debugStream << "CRC calculated: " << crcPayload << std::endl;
        }
        
        if (crcPayload == crcMessage) {
            if (debugStream) {
                *debugStream << "Received message: ";
                for (int i = 0; i < lenMes; i++) {
                    *debugStream << static_cast<int>(message[i]) << " ";
                }
                *debugStream << std::endl;

                *debugStream << "Payload: ";
                for (int i = 0; i < message[1]; i++) {
                    *debugStream << static_cast<int>(payload[i]) << " ";
                }
                *debugStream << std::endl;
            }
            return true;
        }
        
        return false;
    }

    int packSendPayload(uint8_t * payload, int lenPay) {
        uint16_t crcPayload = crc16(payload, lenPay);
        int count = 0;
        uint8_t messageSend[256];

        if (lenPay <= 256) {
            messageSend[count++] = 2;
            messageSend[count++] = static_cast<uint8_t>(lenPay);
        } else {
            messageSend[count++] = 3;
            messageSend[count++] = static_cast<uint8_t>(lenPay >> 8);
            messageSend[count++] = static_cast<uint8_t>(lenPay & 0xFF);
        }

        memcpy(&messageSend[count], payload, lenPay);
        count += lenPay;

        messageSend[count++] = static_cast<uint8_t>(crcPayload >> 8);
        messageSend[count++] = static_cast<uint8_t>(crcPayload & 0xFF);
        messageSend[count++] = 3;

        if (debugStream) {
            *debugStream << "Sending UART package: ";
            for (int i = 0; i < count; i++) {
                *debugStream << static_cast<int>(messageSend[i]) << " ";
            }
            *debugStream << std::endl;
        }

        return serialPort.writeBytes(messageSend, count);
    }

    bool processReadPacket(uint8_t * message) {
        COMM_PACKET_ID packetId;
        int32_t ind = 0;

        packetId = static_cast<COMM_PACKET_ID>(message[0]);
        message++;

        switch (packetId) {
            case COMM_GET_VALUES:
                data.tempFET = buffer_get_float16(message, 10.0, &ind);
                data.tempMotor = buffer_get_float16(message, 10.0, &ind);
                data.avgMotorCurrent = buffer_get_float32(message, 100.0, &ind);
                data.avgInputCurrent = buffer_get_float32(message, 100.0, &ind);
                ind += 8; // 跳过8个字节
                data.dutyCycleNow = buffer_get_float16(message, 1000.0, &ind);
                data.rpm = buffer_get_int32(message, &ind);
                data.inpVoltage = buffer_get_float16(message, 10.0, &ind);
                data.ampHours = buffer_get_float32(message, 10000.0, &ind);
                data.ampHoursCharged = buffer_get_float32(message, 10000.0, &ind);
                ind += 8; // 跳过8个字节
                data.tachometer = buffer_get_int32(message, &ind);
                data.tachometerAbs = buffer_get_int32(message, &ind);
                return true;
            default:
                return false;
        }
    }

    void serialPrint(uint8_t * data, int len) {
        if (debugStream) {
            for (int i = 0; i <= len; i++) {
                *debugStream << static_cast<int>(data[i]) << " ";
            }
            *debugStream << std::endl;
        }
    }

public:
    /// 构造函数，初始化 nunchuck 默认值（X/Y 轴为 127，按钮为 false）及调试流指针为 nullptr
    VescUart() {
        debugStream = nullptr;
    }

    ~VescUart() {
        disconnect();
    }

    /// 连接到 VESC 设备
    /// @param port 串口设备路径（如 "/dev/ttyUSB0"）
    /// @param baudrate 通信波特率（如 115200）
    /// @return 连接成功返回 true，失败返回 false
    bool connect(const std::string& port, unsigned int baudrate) {
        return serialPort.openDevice(port.data(), baudrate) == 1;
    }

    /// 断开与 VESC 设备的连接
    void disconnect() {
        serialPort.closeDevice();
    }

    /// 设置调试输出流，用于打印调试信息
    /// @param stream 指向输出流的指针（如 & std::cout）
    void setDebugStream(std::ostream* stream) {
        debugStream = stream;
    }

    /// 从 VESC 获取状态数据（温度、电流、电压、转速等）
    /// @return 数据获取成功返回 true，失败返回 false
    bool getVescValues(dataPackage & pkg)
    {
        uint8_t command[1] = { COMM_GET_VALUES };
        uint8_t payload[256];

        packSendPayload(command, 1);
        
        // 短暂延迟等待响应
        std::this_thread::sleep_for(std::chrono::microseconds(100));
        
        int lenPayload = receiveUartMessage(payload);

        if (lenPayload > 55) 
        {
            bool flag = processReadPacket(payload);

            if (flag)
            {
                pkg = data;
            }
            
            return flag;
        }
        return false;
    }

    /// @brief 设置电流
    /// @param current 
    void setCurrent(float current) {
        int32_t index = 0;
        uint8_t payload[5];

        payload[index++] = COMM_SET_CURRENT;
        buffer_append_int32(payload, static_cast<int32_t>(current * 1000), &index);

        packSendPayload(payload, 5);
    }

    /// @brief 设置刹车电流
    /// @param brakeCurrent 
    void setBrakeCurrent(float brakeCurrent) {
        int32_t index = 0;
        uint8_t payload[5];

        payload[index++] = COMM_SET_CURRENT_BRAKE;
        buffer_append_int32(payload, static_cast<int32_t>(brakeCurrent * 1000), &index);

        packSendPayload(payload, 5);
    }

    /// @brief 设置转速
    /// @param rpm 
    void setRPM(float rpm) {
        int32_t index = 0;
        uint8_t payload[5];

        payload[index++] = COMM_SET_RPM;
        buffer_append_int32(payload, static_cast<int32_t>(rpm), &index);

        packSendPayload(payload, 5);
    }

    /// @brief 设置占空比
    /// @param duty 
    void setDuty(float duty) {
        int32_t index = 0;
        uint8_t payload[5];

        payload[index++] = COMM_SET_DUTY;
        buffer_append_int32(payload, static_cast<int32_t>(duty * 100000), &index);

        packSendPayload(payload, 5);
    }

    /// @brief 打印状态
    void printVescValues() {
        getVescValues(data);

        if (debugStream) {
            *debugStream << "avgMotorCurrent: " << data.avgMotorCurrent << std::endl;
            *debugStream << "avgInputCurrent: " << data.avgInputCurrent << std::endl;
            *debugStream << "dutyCycleNow: " << data.dutyCycleNow << std::endl;
            *debugStream << "rpm: " << data.rpm << std::endl;
            *debugStream << "inputVoltage: " << data.inpVoltage << std::endl;
            *debugStream << "ampHours: " << data.ampHours << std::endl;
            *debugStream << "ampHoursCharged: " << data.ampHoursCharged << std::endl;
            *debugStream << "tachometer: " << data.tachometer << std::endl;
            *debugStream << "tachometerAbs: " << data.tachometerAbs << std::endl;
        }
    }

    VescUart(const VescUart &) = delete;
    VescUart operator = (const VescUart &) = delete;
};